#include "param.h"

float ksp1, ksi1, ksd1, exps1;
float ksp2, ksi2, ksd2, exps2;
float ksp3, ksi3, ksd3, exps3;
float ksp4, ksi4, ksd4, exps4;

float kdp1, kdi1, kdd1, expd1;
float kdp2, kdi2, kdd2, expd2;
float kdp3, kdi3, kdd3, expd3;
float kdp4, kdi4, kdd4, expd4;
float kgp,kgi,kgd,expg;
int flag1,flag2,flag3,flag4;
bool savep = 0;

String settingprocessor(const String &var)
{
    if (var == "ksp1")
    {
        return String(ksp1);
    }
    else if (var == "ksi1")
    {
        return String(ksi1);
    }
    else if (var == "ksd1")
    {
        return String(ksd1);
    }
    else if (var == "exps1")
    {
        return String(exps1);
    }
    else if (var == "ksp2")
    {
        return String(ksp2);
    }
    else if (var == "ksi2")
    {
        return String(ksi2);
    }
    else if (var == "ksd2")
    {
        return String(ksd2);
    }
    else if (var == "exps2")
    {
        return String(exps2);
    }
    else if (var == "ksp3")
    {
        return String(ksp3);
    }
    else if (var == "ksi3")
    {
        return String(ksi3);
    }
    else if (var == "ksd3")
    {
        return String(ksd3);
    }
    else if (var == "exps3")
    {
        return String(exps3);
    }
    else if (var == "ksp4")
    {
        return String(ksp4);
    }
    else if (var == "ksi4")
    {
        return String(ksi4);
    }
    else if (var == "ksd4")
    {
        return String(ksd4);
    }
    else if (var == "exps4")
    {
        return String(exps4);
    }
    else if (var == "kdp1")
    {
        return String(kdp1);
    }
    else if (var == "kdi1")
    {
        return String(kdi1);
    }
    else if (var == "kdd1")
    {
        return String(kdd1);
    }
    else if (var == "expd1")
    {
        return String(expd1);
    }
    else if (var == "kdp2")
    {
        return String(kdp2);
    }
    else if (var == "kdi2")
    {
        return String(kdi2);
    }
    else if (var == "kdd2")
    {
        return String(kdd2);
    }
    else if (var == "expd2")
    {
        return String(expd2);
    }
    else if (var == "kdp3")
    {
        return String(kdp3);
    }
    else if (var == "kdi3")
    {
        return String(kdi3);
    }
    else if (var == "kdd3")
    {
        return String(kdd3);
    }
    else if (var == "expd3")
    {
        return String(expd3);
    }
    else if (var == "kdp4")
    {
        return String(kdp4);
    }
    else if (var == "kdi4")
    {
        return String(kdi4);
    }
    else if (var == "kdd4")
    {
        return String(kdd4);
    }
    else if (var == "expd4")
    {
        return String(expd4);
    }
    else if (var == "savep")
    {
        return String(savep);
    }
    else if(var=="kgp"){
        return String(kgp);
    }
    else if(var=="kgi"){
        return String(kgi);
    }
    else if(var=="kgd"){
        return String(kgd);
    }
    else if(var=="expg"){
        return String(expg);
    }
    else if(var=="flag1"){
        return String(flag1);
    }
    else if(var=="flag2"){
        return String(flag2);
    }
    else if(var=="flag3"){
        return String(flag3);
    }
    else if(var=="flag4"){
        return String(flag4);
    }
    return String();
}
void Param::begin_server()

{
    SPIFFS.begin();
    AsyncWebServer::on("/", HTTP_GET, [](AsyncWebServerRequest *request)
                       { request->send(SPIFFS, "/setting.html", String(), false, settingprocessor); });
    AsyncWebServer::on("/setting.html", HTTP_GET, [](AsyncWebServerRequest *request)
                       { request->send(SPIFFS, "/setting.html", String(), false, settingprocessor); });
    AsyncWebServer::on("/setting.html", HTTP_POST, [](AsyncWebServerRequest *request)
                       {

            int params = request->params();
            
            for (int i = 0; i < params; i++)
            {
                AsyncWebParameter *p = request->getParam(i);
                if (p->isPost())
                {

                    Serial.printf("get:  %s:%s\n", p->name().c_str(), p->value().c_str());
                    if(p->name()=="ksp1") {
                        ksp1 = p->value().toFloat();
                        speed_pid1.kp=ksp1;
                  
                    }
                    else if(p->name()=="ksi1"){
                        ksi1 = p->value().toFloat();
                        speed_pid1.ki=ksi1;
                    }
                    else if(p->name()=="ksd1"){
                        ksd1 = p->value().toFloat();
                        speed_pid1.kd=ksd1;
                    }
                    if(p->name()=="ksp2") {
                        ksp2 = p->value().toFloat();
                        speed_pid2.kp=ksp2;
                    }
                    else if(p->name()=="ksi2"){
                        ksi2 = p->value().toFloat();
                        speed_pid2.ki=ksi2;
                    }
                    else if(p->name()=="ksd2"){
                        ksd2 = p->value().toFloat();
                        speed_pid2.kd=ksd2;
                    }
                    if(p->name()=="ksp3") {
                        ksp3 = p->value().toFloat();
                        speed_pid3.kp=ksp3;
                    }
                    else if(p->name()=="ksi3"){
                        ksi3 = p->value().toFloat();
                        speed_pid3.ki=ksi3;
                    }
                    else if(p->name()=="ksd3"){
                        ksd3 = p->value().toFloat();
                        speed_pid3.kd=ksd3;
                    }
                    if(p->name()=="ksp4") {
                        ksp4 = p->value().toFloat();
                        speed_pid4.kp=ksp4;
                    }
                    else if(p->name()=="ksi4"){
                        ksi4 = p->value().toFloat();
                        speed_pid4.ki=ksi4;
                    }
                    else if(p->name()=="ksd4"){
                        ksd4 = p->value().toFloat();
                        speed_pid4.kd=ksd4;
                    }
                    
                    if(p->name()=="kdp1") {
                        kdp1 = p->value().toFloat();
                        dis_pid1.kp=kdp1;
                    }
                    else if(p->name()=="kdi1"){
                        kdi1 = p->value().toFloat();
                        dis_pid1.ki=kdi1;
                    }
                    else if(p->name()=="kdd1"){
                        kdd1 = p->value().toFloat();
                        dis_pid1.kd=kdd1;
                    }
                    if(p->name()=="kdp2") {
                        kdp2 = p->value().toFloat();
                        dis_pid2.kp=kdp2;
                    }
                    else if(p->name()=="kdi2"){
                        kdi2 = p->value().toFloat();
                        dis_pid2.ki=kdi2;
                    }
                    else if(p->name()=="kdd2"){
                        kdd2 = p->value().toFloat();
                        dis_pid2.kd=kdd2;
                    }
                    if(p->name()=="kdp3") {
                        kdp3 = p->value().toFloat();
                        dis_pid3.kp=kdp3;
                    }
                    else if(p->name()=="kdi3"){
                        kdi3 = p->value().toFloat();
                        dis_pid3.ki=kdi3;
                    }
                    else if(p->name()=="kdd3"){
                        kdd3 = p->value().toFloat();
                        dis_pid3.kd=kdd3;
                    }
                    if(p->name()=="kdp4") {
                        kdp4 = p->value().toFloat();
                        dis_pid4.kp=kdp4;
                    }
                    else if(p->name()=="kdi4"){
                        kdi4 = p->value().toFloat();
                        dis_pid4.ki=kdi4;
                    }
                    else if(p->name()=="kdd4"){
                        kdd4 = p->value().toFloat();
                        dis_pid4.kd=kdd4;
                    }
                    if(p->name()=="expd1") {
                        expd1 = p->value().toFloat();
                        dis_pid1.setpoint=expd1;
                    }
                    if(p->name()=="expd2") {
                        expd2 = p->value().toFloat();
                        dis_pid2.setpoint=expd2;
                    }
                    if(p->name()=="expd3") {
                        expd3 = p->value().toFloat();
                        dis_pid3.setpoint=expd3;
                    }
                    if(p->name()=="expd4") {
                        expd4 = p->value().toFloat();
                        dis_pid4.setpoint=expd4;
                    }
                    if(p->name()=="exps1") {
                        exps1 = p->value().toFloat();
                        ros_expect_speed_data.exps1=exps1;
                       
                    }
                    if(p->name()=="exps2") {
                        exps2 = p->value().toFloat();
                       
                        ros_expect_speed_data.exps2 = exps2;
                    }
                    if(p->name()=="exps3") {
                        exps3 = p->value().toFloat();
                        ros_expect_speed_data.exps3 = exps3;
                    }
                    if(p->name()=="exps4") {
                        exps4 = p->value().toFloat();
                        ros_expect_speed_data.exps4 = exps4;
                    }
                    if(p->name()=="savep") {
                        savep = p->value().toFloat();
                        if(savep)save_all_param();
                    }
                    if(p->name()=="kgp"){
                        kgp = p->value().toFloat();
                        gyro_pid.kp=kgp;
                    }
                    else if(p->name()=="kgi"){
                        kgi = p->value().toFloat();
                        gyro_pid.ki=kgi;
                    }
                    else if(p->name()=="kgd"){
                        kgd = p->value().toFloat();
                        gyro_pid.kd=kgd;
                    }
                    else if(p->name()=="expg"){
                        expg = p->value().toFloat();
                        gyro_pid.setpoint=expg;
                    }
                    if(p->name()=="flag1"){
                        flag1 = p->value().toInt();
                        
                    }
                    if(p->name()=="flag2"){
                        flag2 = p->value().toInt();
                          
                    }
                    if(p->name()=="flag3"){
                        flag3 = p->value().toInt();
                 
                    }
                    if(p->name()=="flag4"){
                        flag4 = p->value().toInt();
                        
                    }
                }
            } ; request->send(SPIFFS, "/setting.html", String(), false, settingprocessor); });
    AsyncWebServer::begin();
    Serial.println("Server started at http://");
    Serial.println(WiFi.localIP());
}
void Param::recover_param(){
    ksp1=speed_pid1.kp;
    ksi1=speed_pid1.ki;
    ksd1=speed_pid1.kd;
    ksp2=speed_pid2.kp;
    ksi2=speed_pid2.ki;
    ksd2=speed_pid2.kd;
    ksp3=speed_pid3.kp;
    ksi3=speed_pid3.ki;
    ksd3=speed_pid3.kd;
    ksp4=speed_pid4.kp;
    ksi4=speed_pid4.ki;
    ksd4=speed_pid4.kd;
    kdp1=dis_pid1.kp;
    kdi1=dis_pid1.ki;
    kdd1=dis_pid1.kd;
    kdp2=dis_pid2.kp;
    kdi2=dis_pid2.ki;
    kdd2=dis_pid2.kd;
    kdp3=dis_pid3.kp;
    kdi3=dis_pid3.ki;
    kdd3=dis_pid3.kd;
    kdp4=dis_pid4.kp;
    kdi4=dis_pid4.ki;
    kdd4=dis_pid4.kd;
    kgp=gyro_pid.kp;
    kgi=gyro_pid.ki;
    kgd=gyro_pid.kd;
    // get_flag(&flag1,&flag2,&flag3,&flag4);
}

void Param::save_flag(){
    File f = SPIFFS.open("/flag.txt", "w");
    if (!f) {
        Serial.println("file open failed");
        return;
    }
    f.print(flag1);
    f.print(" ");
    f.print(flag2);
    f.print(" ");
    f.print(flag3);
    f.print(" ");
    f.print(flag4);
    f.close();
}
void Param::get_flag(int *flag1,int *flag2,int *flag3,int *flag4){
    File f = SPIFFS.open("/flag.txt", "r");
    if (!f) {
        Serial.println("file open failed");
        return;
    }
    String str = f.readString();
    f.close();
    int i=0;
    int j=0;
    int k=0;
    int l=0;
    while(str[i]!=' '){
        i++;
    }
    while(str[j]!=' '){
        j++;
    }
    while(str[k]!=' '){
        k++;
    }
    while(str[l]!=' '){
        l++;
    }
    *flag1=str.substring(0,i).toInt();
    *flag2=str.substring(i+1,j).toInt();
    *flag3=str.substring(j+1,k).toInt();
    *flag4=str.substring(k+1,l).toInt();
}